/*!
 * MindPlus
 * pico
 *
 */
#define LEFT_MOTOR_PWM 10
#define LEFT_MOTOR_DIR1 11
#define LEFT_MOTOR_DIR2 12
#define RIGHT_MOTOR_PWM 15
#define RIGHT_MOTOR_DIR1 14
#define RIGHT_MOTOR_DIR2 13
#define BUZZER_PIN 18
#define SPEED_STEP 35
#define MIN_SPEED 80
#define MAX_SPEED 255
#define NUM_SPEED_LEVELS 6
#define SPIN_TURN_SPEED 80
#define DIAGONAL_SLOW_FACTOR 0.4

// 新增：用于补偿左右轮转速差异的全局变量
int rightMotorSpeedOffset = -15; // 可以根据实际情况修改这个值，正数增加右轮速度，负数降低右轮速度

String comdata = "";
volatile float ywt_variable_speed_n = 150.0;

void beep(int frequency, int duration) {
  tone(BUZZER_PIN, frequency, duration);
  delay(duration + 50);
  noTone(BUZZER_PIN);
}

void indicateSpeedChange(bool isAccelerate) {
  if (isAccelerate) {
    beep(1200, 100);
    delay(50);
    beep(1200, 100);
  } else {
    beep(300, 200);
    delay(50);
    beep(300, 200);
  }
}

void setLeftMotor(int speed, int dir1, int dir2) {
    analogWrite(LEFT_MOTOR_PWM, speed);
    digitalWrite(LEFT_MOTOR_DIR1, dir1);
    digitalWrite(LEFT_MOTOR_DIR2, dir2);
    Serial.print("L_MOTOR: S=");
    Serial.print(speed);
    Serial.print(",D1=");
    Serial.print(dir1);
    Serial.print(",D2=");
    Serial.println(dir2);
}

void setRightMotor(int speed, int dir1, int dir2) {
    analogWrite(RIGHT_MOTOR_PWM, speed);
    digitalWrite(RIGHT_MOTOR_DIR1, dir1);
    digitalWrite(RIGHT_MOTOR_DIR2, dir2);
    Serial.print("R_MOTOR: S=");
    Serial.print(speed);
    Serial.print(",D1=");
    Serial.print(dir1);
    Serial.print(",D2=");
    Serial.println(dir2);
}

void stopMotors() {
    setLeftMotor(0, 0, 0);
    setRightMotor(0, 0, 0);
    Serial.println("MOTORS_STOPPED");
}

void moveForward() {
    Serial.println("CMD: Forward");
    int baseSpeed = (int)ywt_variable_speed_n;
    // 应用补偿值
    int leftSpeed = baseSpeed;
    int rightSpeed = baseSpeed + rightMotorSpeedOffset;
    // 确保速度在有效范围内
    leftSpeed = constrain(leftSpeed, 0, MAX_SPEED);
    rightSpeed = constrain(rightSpeed, 0, MAX_SPEED);
    
    setLeftMotor(leftSpeed, 1, 0);
    setRightMotor(rightSpeed, 1, 0);
}

void moveBackward() {
    Serial.println("CMD: Backward");
    int baseSpeed = (int)ywt_variable_speed_n;
    // 应用补偿值
    int leftSpeed = baseSpeed;
    int rightSpeed = baseSpeed + rightMotorSpeedOffset;
    // 确保速度在有效范围内
    leftSpeed = constrain(leftSpeed, 0, MAX_SPEED);
    rightSpeed = constrain(rightSpeed, 0, MAX_SPEED);
    
    setLeftMotor(leftSpeed, 0, 1);
    setRightMotor(rightSpeed, 0, 1);
}

void fixedTurnLeft() {
    Serial.println("CMD: Fixed Left (Spin)");
    int baseSpeed = SPIN_TURN_SPEED;
    // 应用补偿值（如果需要）
    int leftSpeed = baseSpeed;
    int rightSpeed = baseSpeed + rightMotorSpeedOffset;
    // 确保速度在有效范围内
    leftSpeed = constrain(leftSpeed, 0, MAX_SPEED);
    rightSpeed = constrain(rightSpeed, 0, MAX_SPEED);

    setLeftMotor(leftSpeed, 0, 1); // 左轮反转
    setRightMotor(rightSpeed, 1, 0); // 右轮正转
}

void fixedTurnRight() {
    Serial.println("CMD: Fixed Right (Spin)");
    int baseSpeed = SPIN_TURN_SPEED;
    // 应用补偿值（如果需要）
    int leftSpeed = baseSpeed;
    int rightSpeed = baseSpeed + rightMotorSpeedOffset;
    // 确保速度在有效范围内
    leftSpeed = constrain(leftSpeed, 0, MAX_SPEED);
    rightSpeed = constrain(rightSpeed, 0, MAX_SPEED);

    setLeftMotor(leftSpeed, 1, 0); // 左轮正转
    setRightMotor(rightSpeed, 0, 1); // 右轮反转
}

void moveForwardLeft() {
    Serial.println("CMD: Forward Left");
    int fastSpeed = (int)ywt_variable_speed_n;
    int slowSpeed = (int)(fastSpeed * DIAGONAL_SLOW_FACTOR);
    slowSpeed = max(MIN_SPEED, slowSpeed);

    // 对斜向移动也应用补偿，但需要更仔细考虑
    // 这里假设我们仍然想保持相对的快慢关系，但对右轮整体加补偿
    int adjustedLeftFast = fastSpeed;
    int adjustedRightSlow = slowSpeed + rightMotorSpeedOffset;
    
    // 确保速度在有效范围内
    adjustedLeftFast = constrain(adjustedLeftFast, 0, MAX_SPEED);
    adjustedRightSlow = constrain(adjustedRightSlow, 0, MAX_SPEED);
    // 也要确保调整后仍然符合斜向移动的逻辑（快轮比慢轮快）
    if (adjustedLeftFast < adjustedRightSlow) {
        // 如果补偿导致快轮变慢，可能需要特殊处理或禁用补偿
        // 这里简单地重新设置为原始斜向速度，或者可以考虑只补偿快轮
        setLeftMotor(fastSpeed, 1, 0); // 左轮（快）
        setRightMotor(slowSpeed, 1, 0); // 右轮（慢）
    } else {
         setLeftMotor(adjustedLeftFast, 1, 0); // 左轮（快）
         setRightMotor(adjustedRightSlow, 1, 0); // 右轮（慢）
    }
}

void moveForwardRight() {
    Serial.println("CMD: Forward Right");
    int fastSpeed = (int)ywt_variable_speed_n;
    int slowSpeed = (int)(fastSpeed * DIAGONAL_SLOW_FACTOR);
    slowSpeed = max(MIN_SPEED, slowSpeed);

    // 对斜向移动也应用补偿，但需要更仔细考虑
    // 这里假设我们仍然想保持相对的快慢关系，但对右轮整体加补偿
    int adjustedLeftSlow = slowSpeed;
    int adjustedRightFast = fastSpeed + rightMotorSpeedOffset;
    
    // 确保速度在有效范围内
    adjustedLeftSlow = constrain(adjustedLeftSlow, 0, MAX_SPEED);
    adjustedRightFast = constrain(adjustedRightFast, 0, MAX_SPEED);
    // 也要确保调整后仍然符合斜向移动的逻辑（快轮比慢轮快）
    if (adjustedRightFast < adjustedLeftSlow) {
        // 如果补偿导致快轮变慢，可能需要特殊处理或禁用补偿
        // 这里简单地重新设置为原始斜向速度，或者可以考虑只补偿快轮
        setLeftMotor(slowSpeed, 1, 0); // 左轮（慢）
        setRightMotor(fastSpeed, 1, 0); // 右轮（快）
    } else {
         setLeftMotor(adjustedLeftSlow, 1, 0); // 左轮（慢）
         setRightMotor(adjustedRightFast, 1, 0); // 右轮（快）
    }
}

void moveBackwardLeft() {
    Serial.println("CMD: Backward Left");
    int fastSpeed = (int)ywt_variable_speed_n;
    int slowSpeed = (int)(fastSpeed * DIAGONAL_SLOW_FACTOR);
    slowSpeed = max(MIN_SPEED, slowSpeed);

    // 对斜向移动也应用补偿，但需要更仔细考虑
    // 这里假设我们仍然想保持相对的快慢关系，但对右轮整体加补偿
    int adjustedLeftFast = fastSpeed;
    int adjustedRightSlow = slowSpeed + rightMotorSpeedOffset;
    
    // 确保速度在有效范围内
    adjustedLeftFast = constrain(adjustedLeftFast, 0, MAX_SPEED);
    adjustedRightSlow = constrain(adjustedRightSlow, 0, MAX_SPEED);
    // 也要确保调整后仍然符合斜向移动的逻辑（快轮比慢轮快）
    if (adjustedLeftFast < adjustedRightSlow) {
        // 如果补偿导致快轮变慢，可能需要特殊处理或禁用补偿
        // 这里简单地重新设置为原始斜向速度，或者可以考虑只补偿快轮
        setLeftMotor(fastSpeed, 0, 1); // 左轮（快）
        setRightMotor(slowSpeed, 0, 1); // 右轮（慢）
    } else {
         setLeftMotor(adjustedLeftFast, 0, 1); // 左轮（快）
         setRightMotor(adjustedRightSlow, 0, 1); // 右轮（慢）
    }
}

void moveBackwardRight() {
    Serial.println("CMD: Backward Right");
    int fastSpeed = (int)ywt_variable_speed_n;
    int slowSpeed = (int)(fastSpeed * DIAGONAL_SLOW_FACTOR);
    slowSpeed = max(MIN_SPEED, slowSpeed);

    // 对斜向移动也应用补偿，但需要更仔细考虑
    // 这里假设我们仍然想保持相对的快慢关系，但对右轮整体加补偿
    int adjustedLeftSlow = slowSpeed;
    int adjustedRightFast = fastSpeed + rightMotorSpeedOffset;
    
    // 确保速度在有效范围内
    adjustedLeftSlow = constrain(adjustedLeftSlow, 0, MAX_SPEED);
    adjustedRightFast = constrain(adjustedRightFast, 0, MAX_SPEED);
    // 也要确保调整后仍然符合斜向移动的逻辑（快轮比慢轮快）
    if (adjustedRightFast < adjustedLeftSlow) {
        // 如果补偿导致快轮变慢，可能需要特殊处理或禁用补偿
        // 这里简单地重新设置为原始斜向速度，或者可以考虑只补偿快轮
        setLeftMotor(slowSpeed, 0, 1); // 左轮（慢）
        setRightMotor(fastSpeed, 0, 1); // 右轮（快）
    } else {
         setLeftMotor(adjustedLeftSlow, 0, 1); // 左轮（慢）
         setRightMotor(adjustedRightFast, 0, 1); // 右轮（快）
    }
}

// 主程序开始
void setup() {
    Serial.begin(9600);
    delay(2000);
    Serial.println("--- Robot Car Control Started ---");
    Serial.print("Initial Speed Variable: ");
    Serial.println(ywt_variable_speed_n);
    Serial.print("Speed Step: ");
    Serial.println(SPEED_STEP);
    Serial.print("Min Speed: ");
    Serial.println(MIN_SPEED);
    Serial.print("Max Speed: ");
    Serial.println(MAX_SPEED);
    Serial.print("Number of Speed Levels: ");
    Serial.println(NUM_SPEED_LEVELS);
    Serial.print("Fixed Spin Turn Speed: ");
    Serial.println(SPIN_TURN_SPEED);
    Serial.print("Diagonal Slow Factor: ");
    Serial.println(DIAGONAL_SLOW_FACTOR);
    Serial.print("Right Motor Speed Offset: ");
    Serial.println(rightMotorSpeedOffset); // 打印补偿值
    Serial.println("Debug output via USB Serial (9600 baud)");
    pinMode(LEFT_MOTOR_PWM, OUTPUT);
    pinMode(LEFT_MOTOR_DIR1, OUTPUT);
    pinMode(LEFT_MOTOR_DIR2, OUTPUT);
    pinMode(RIGHT_MOTOR_PWM, OUTPUT);
    pinMode(RIGHT_MOTOR_DIR1, OUTPUT);
    pinMode(RIGHT_MOTOR_DIR2, OUTPUT);
    Serial.println("Motor pins initialized.");
    pinMode(BUZZER_PIN, OUTPUT);
    Serial.println("Buzzer pin initialized.");
    Serial2.begin(115200);
    Serial.println("Bluetooth Serial2 started (115200 baud)");
    stopMotors();
    Serial.println("--- Setup Complete ---");
}

void loop() {
    while (Serial2.available() > 0) {
        char receivedChar = Serial2.read();
        comdata += receivedChar;
        delay(2);
    }
    if (comdata.length() > 0) {
        Serial.print("Received Command: '");
        Serial.print(comdata);
        Serial.println("'");
        comdata.trim();
        if (comdata == "BLE_Forward") {
            moveForward();
        }
        else if (comdata == "BLE_Backward") {
            moveBackward();
        }
        else if (comdata == "BLE_C") {
            fixedTurnLeft();
        }
        else if (comdata == "BLE_D") {
            fixedTurnRight();
        }
        else if (comdata == "BLE_Forward_Left") {
            moveForwardLeft();
        }
        else if (comdata == "BLE_Forward_Right") {
            moveForwardRight();
        }
        else if (comdata == "BLE_Backward_Left") {
            moveBackwardLeft();
        }
        else if (comdata == "BLE_Backward_Right") {
            moveBackwardRight();
        }
        else if (comdata == "BLE_Stop") {
            stopMotors();
        }
        else if ((comdata == "BLE_Accelerate") && (ywt_variable_speed_n <= (MAX_SPEED - SPEED_STEP))) {
            Serial.print("CMD: Accelerate. Old Speed Var: ");
            Serial.println(ywt_variable_speed_n);
            ywt_variable_speed_n += SPEED_STEP;
            Serial.print("CMD: Accelerate. New Speed Var: ");
            Serial.println(ywt_variable_speed_n);
            indicateSpeedChange(true);
        }
        else if ((comdata == "BLE_Moderate") && (ywt_variable_speed_n >= (MIN_SPEED + SPEED_STEP))) {
            Serial.print("CMD: Moderate. Old Speed Var: ");
            Serial.println(ywt_variable_speed_n);
            ywt_variable_speed_n -= SPEED_STEP;
            Serial.print("CMD: Deccelerate. New Speed Var: ");
            Serial.println(ywt_variable_speed_n);
            indicateSpeedChange(false);
        }
        else {
            Serial.println("Unknown command");
            Serial.print(comdata);
            Serial.println("'");
        }
        comdata = "";
    }
    delay(10);
}



